#include <geometry_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <gflags/gflags.h>
#include <nav_msgs/Odometry.h>
#include <functional>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
#include <sophus/so3.hpp>
#include <std_msgs/Float64.h>
#include <fstream>

DEFINE_string(true_topic, "/vrpn_client_node/uav1/pose", "This is dong pu true pose");
DEFINE_string(odom_topic, "/baton/stereo3/odometry", "This is odom pose");
DEFINE_double(gaptime, 1, "calculate gap time");
DEFINE_bool(pub_error, true, "publish error");

double all_error = 0.0;
double all_transform_error = 0.0;
double all_translation_error = 0.0;
double count = 0;

Eigen::Vector3d true_pose = Eigen::Vector3d::Zero();
Eigen::Vector3d ltrue_pose = Eigen::Vector3d::Zero();

Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
Eigen::Vector3d lodom_pose = Eigen::Vector3d::Zero();

Eigen::Matrix3d true_R = Eigen::Matrix3d::Identity();
Eigen::Matrix3d ltrue_R = Eigen::Matrix3d::Identity();

Eigen::Matrix3d odom_R = Eigen::Matrix3d::Identity();
Eigen::Matrix3d lodom_R = Eigen::Matrix3d::Identity();

Eigen::Matrix3d Rt2o ;

bool flag = false;
bool firsttrue = true;
bool firstodom = true;
ros::Time time1;
ros::Time time2;
ros::Time currenttime;

ros::Publisher err_x_pub;
ros::Publisher err_y_pub;
ros::Publisher err_z_pub;
ros::Publisher translation_error_pub;

ros::Publisher err_roll_pub;
ros::Publisher err_pitch_pub;
ros::Publisher err_yaw_pub;
ros::Publisher rotation_error_pub;


void dongpucallback(const geometry_msgs::PoseStamped::ConstPtr& pose) {
    // 处理 true pose 数据
    double x = pose->pose.position.x;
    double y = pose->pose.position.y;
    double z = pose->pose.position.z;

    double qw = pose->pose.orientation.w;
    double qx = pose->pose.orientation.x;
    double qy = pose->pose.orientation.y;
    double qz = pose->pose.orientation.z;

    true_pose << x, y, z;
    Eigen::Quaterniond q(qw, qx, qy, qz);
    q.normalize();
    true_R = q.toRotationMatrix();
    if (firsttrue) {
        ROS_WARN("Received first true pose data!");
        firsttrue = false;
        time1 = pose->header.stamp;
        ltrue_pose = true_pose;
        ltrue_R = true_R;
        return;
    }

    time1 = pose->header.stamp;
    
    // ros::Time nowtime = pose->header.stamp;
    // double t = (nowtime - time1).toSec();
    // if (t >= FLAGS_gaptime) {
    //     time2 = nowtime;
    //     flag = true;
    //     count++;
    // }
}

void odomcallback(const nav_msgs::Odometry::ConstPtr& pose) {
    // 处理 odom pose 数据

    // 旋转矩阵RI2C
    // Eigen::Matrix3d  RI2C;
    // RI2C = Eigen::AngleAxisd(M_PI * 0.5,Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(- M_PI * 0.5,Eigen::Vector3d::UnitZ());

    if(firsttrue) {
        ROS_WARN("Wait odom init!");        
        return;
    }

    double x = pose->pose.pose.position.x;
    double y = pose->pose.pose.position.y;
    double z = pose->pose.pose.position.z;

    double qw = pose->pose.pose.orientation.w;
    double qx = pose->pose.pose.orientation.x;
    double qy = pose->pose.pose.orientation.y;
    double qz = pose->pose.pose.orientation.z;

    odom_pose << x, y, z;
    Eigen::Quaterniond q(qw, qx, qy, qz);
    q.normalize();
    Eigen::Vector3d p1(pose->pose.pose.position.x,pose->pose.pose.position.y,pose->pose.pose.position.z);
    p1 = ltrue_R * p1 + ltrue_pose;

    odom_R = q.toRotationMatrix();
    odom_R = ltrue_R * odom_R;
    if (firstodom) {
        ROS_WARN("Received first true odom data!");
        firstodom = false;
        lodom_pose = odom_pose;
        lodom_R = odom_R;
        Rt2o = ltrue_R.transpose() * lodom_R;
        return;
    }


    time2 = pose->header.stamp;

    if(abs((time2 - time1).toSec()) < 0.001) {
        // double err_x = odom_pose.x() - true_pose.x();
        // double err_y = odom_pose.y() - true_pose.y();
        // double err_z = odom_pose.z() - true_pose.z();
        // double err_tanslation = sqrt(err_x * err_x + err_y * err_y + err_z * err_z);
        double err_x = p1(0) - true_pose.x();
        double err_y = p1(1) - true_pose.y();
        double err_z = p1(2) - true_pose.z();
        double err_tanslation = sqrt(err_x * err_x + err_y * err_y + err_z * err_z);

        // Eigen::Matrix3d delta_R = Rt2o * odom_R * true_R.transpose();
        // Eigen::Vector3d euler = delta_R.eulerAngles( 2, 1, 0);
        Eigen::Vector3d euler = odom_R.eulerAngles( 2, 1, 0) - true_R.eulerAngles( 2, 1, 0);
        double err_roll = euler(2);
        double err_pitch = euler(1);
        double err_yaw = euler(0);
        err_yaw = std::fmod(err_yaw + M_PI, 2 * M_PI) - M_PI;
        err_pitch = std::fmod(err_pitch + M_PI, 2 * M_PI) - M_PI;
        err_roll = std::fmod(err_roll + M_PI, 2 * M_PI) - M_PI;
        if(abs(err_roll) > 0.9 * M_PI || abs(err_pitch) > 0.9 * M_PI || abs(err_yaw) > 0.9 * M_PI) return;
        // err_roll = std::fmod(err_roll + M_PI, 2 * M_PI) - M_PI;
        // err_pitch = std::fmod(err_pitch + M_PI, 2 * M_PI) - M_PI;
        // err_yaw = std::fmod(err_yaw + M_PI, 2 * M_PI) - M_PI;

        double err_transform = sqrt(err_roll * err_roll + err_pitch * err_pitch + err_yaw * err_yaw);

        count++;
        all_translation_error = all_translation_error * (count - 1) / (count) + err_tanslation / count;
        all_transform_error = all_transform_error * (count - 1) / (count) + err_transform / count;
        all_error = all_translation_error + all_transform_error;

        if(FLAGS_pub_error){
            std_msgs::Float64 err_x_msg;
            err_x_msg.data = err_x;
            err_x_pub.publish(err_x_msg);
            std_msgs::Float64 err_y_msg;
            err_y_msg.data = err_y;
            err_y_pub.publish(err_y_msg);
            std_msgs::Float64 err_z_msg;
            err_z_msg.data = err_z;
            err_z_pub.publish(err_z_msg);

            std_msgs::Float64 err_roll_msg;
            err_roll_msg.data = err_roll;
            err_roll_pub.publish(err_roll_msg);
            std_msgs::Float64 err_pitch_msg;
            err_pitch_msg.data = err_pitch;
            err_pitch_pub.publish(err_pitch_msg);
            std_msgs::Float64 err_yaw_msg;
            err_yaw_msg.data = err_yaw;
            err_yaw_pub.publish(err_yaw_msg);
            
            std_msgs::Float64 translation_error_msg;
            translation_error_msg.data = err_tanslation;
            translation_error_pub.publish(translation_error_msg);
            std_msgs::Float64 transform_error_msg;
            transform_error_msg.data = err_transform;
            rotation_error_pub.publish(transform_error_msg);
            

        }

        if(time2 - currenttime > ros::Duration(1.0)){
            currenttime = time2;
            std::cout << std::fixed << std::setprecision(6);
            std::cout << "\033[32m"<< " err_x     " << "             err_y     " << "     err_z "<< "\n" 
            << err_x <<"              "<< err_y <<"       "<< err_z <<"\033[0m"<<std::endl;
            std::cout << "\033[32m"<< " err_yaw             " << " err_pitch " << "      err_roll "<< "\n"
            << err_yaw <<"              "<< err_pitch <<"       "<< err_roll <<"\033[0m"<<std::endl;
            std::cout << "\033[32m"<< " translation_error " << " transform_error " << " all_error "<< "\n" 
            << all_translation_error <<"               "<< all_transform_error <<"       "<< all_error <<"\033[0m"<<"\n"<<std::endl;
        }
    }
    return;
 //////////////////////////////////////////////////////////END
// std::cout<<"\033[32m"<< "euler_Z " << "euler_Y " << "euler_X "<<"\n" << euler_angles(0) * 180 / M_PI << " " << euler_angles(1) * 180 / M_PI << " " << euler_angles(2) * 180 / M_PI <<"\033[0m"<<std::endl;
//     if (flag) {
//         ROS_WARN("Received odom pose data!");
//         // 计算误差
//         // 1. 平移误差
//         double t = (time1 - currenttime).toSec();
//         double delt = (true_pose - ltrue_pose).norm();

//         double vx = pose->twist.twist.linear.x;
//         double vy = pose->twist.twist.linear.y;
//         double vz = pose->twist.twist.linear.z;

//         double wx = pose->twist.twist.angular.x;
//         double wy = pose->twist.twist.angular.y;
//         double wz = pose->twist.twist.angular.z;

//         odom_pose = odom_pose + t * Eigen::Vector3d(vx, vy, vz);
//         double odelt = (odom_pose - lodom_pose).norm();

//         double trans_error = (odelt - delt) * (odelt - delt);
//         ROS_WARN("The translate error is :%f", trans_error);

//         // 2. 旋转误差
//         Sophus::SO3d delR = Sophus::SO3d(ltrue_R).inverse() * Sophus::SO3d(true_R);
//         Eigen::Quaterniond delta_q(1, wx * 1 / 2 * t, wy * 1 / 2 * t, wz * 1 / 2 * t);
//         q = q * delta_q;
//         q.normalize();
//         odom_R = q.toRotationMatrix();
//         Sophus::SO3d odelR = Sophus::SO3d(lodom_R).inverse() * Sophus::SO3d(odom_R);
//         Sophus::SO3d error_R = odelR.inverse() * delR;
//         Eigen::Vector3d error_axis_angle = error_R.log();
//         double rotation_error = error_axis_angle.norm();
//         ROS_WARN("The transform error is : %f", rotation_error);

//         // 更新总误差
//         all_error = all_error * (count - 1) / (count) + (trans_error + rotation_error) / count;
//         ROS_WARN("The all error is : %f", all_error);

//         // 发布误差数据
//         std_msgs::Float64 rotation_error_msg;
//         rotation_error_msg.data = rotation_error;
//         rotation_error_pub.publish(rotation_error_msg);

//         std_msgs::Float64 curr_error_msg;
//         curr_error_msg.data = trans_error;
//         curr_error_pub.publish(curr_error_msg);

//         std_msgs::Float64 all_error_msg;
//         all_error_msg.data = all_error;
//         all_error_pub.publish(all_error_msg);

//         // 更新状态
//         lodom_pose = odom_pose;
//         lodom_R = odom_R;
//         ltrue_pose = true_pose;
//         ltrue_R = true_R;
//         time1 = time2;
//         flag = false;
//     }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "trajectory_err");
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    ros::NodeHandle nh;

    // 定义发布者
    err_x_pub = nh.advertise<std_msgs::Float64>("/err_x_pub", 10);
    err_y_pub = nh.advertise<std_msgs::Float64>("/err_y_pub", 10);
    err_z_pub = nh.advertise<std_msgs::Float64>("/err_z_pub", 10);
    err_roll_pub = nh.advertise<std_msgs::Float64>("/err_roll_pub", 10);
    err_pitch_pub = nh.advertise<std_msgs::Float64>("/err_pitch_pub", 10);
    err_yaw_pub = nh.advertise<std_msgs::Float64>("/err_yaw_pub", 10);
    translation_error_pub = nh.advertise<std_msgs::Float64>("/translation_error", 10);
    rotation_error_pub = nh.advertise<std_msgs::Float64>("/rotation_error", 10);

    ROS_WARN("evo start ");
    ros::Subscriber dongpu = nh.subscribe(FLAGS_true_topic, 10, dongpucallback);
    ros::Subscriber odompose = nh.subscribe(FLAGS_odom_topic, 10, odomcallback);

    ros::spin();

    return 0;
}